#!/usr/bin/python
# READY FOR MIT
"""
Blink LEDs to indicate sailing state
\n
Publishes: messages that encode a colour in an Int32
\n
Subscribes: sailing state
"""

import rospy
from std_msgs.msg import Int32, String


class Blink_on_sailing_state():
    def __init__(self):
        self.led_blink = rospy.Publisher('led_blink', Int32, queue_size=10)

        rospy.init_node("debugging_blink_on_sailing_state", anonymous=True)
        rospy.Subscriber('sailing_state', String, self.update_sailing_state)
        self.sailing_state = "normal"
        self.rate = rospy.Rate(rospy.get_param("config/rate"))
        self.blink_publisher()


    def update_sailing_state(self, msg):
        self.sailing_state = msg.data


    def blink_publisher(self):

        while not rospy.is_shutdown():

            if self.sailing_state == "switch_to_port_tack":
                color = 255*300*300 # red
                self.led_blink.publish(color)

            elif self.sailing_state == "switch_to_stbd_tack":
                color = 255*300 # green
                self.led_blink.publish(color)

            self.rate.sleep()


if __name__ == '__main__':
    try:
        Blink_on_sailing_state()
    except rospy.ROSInterruptException:
        pass
